/**
 * @file task_action_client.cpp
 * 说明：封装任务执行客户端节点，用于发送任务到任务执行服务器。
 */
#include "task_action_client.h"
#include <QDateTime>
#include <iomanip>
#include <sstream>

TaskClientNode::TaskClientNode(const std::string& namespace_name)
    : QObject(), rclcpp::Node("rms_task_client"), namespace_name_(namespace_name), client_created_(false)
{
    // 初始化日志记录器
    logger_ = GLOBAL_LOGGER;
    LOG_INFO(" Constructor called with namespace: {}", namespace_name);
}

/**
 * @brief 清理动作客户端资源
 * 
 * 释放动作客户端并重置创建状态标志。当需要重新初始化或销毁
 * 客户端节点时调用此函数来确保资源得到正确释放。
 */
void TaskClientNode::cleanup() {
    if (action_client_) {
        action_client_.reset();
    }
    client_created_ = false;
}

/**
 * @brief 析构函数，负责清理客户端节点资源
 * 
 * 在对象销毁时自动调用，确保释放动作客户端相关资源。
 * 通过调用cleanup()函数完成实际的资源清理工作。
 */
TaskClientNode::~TaskClientNode() {
    cleanup();
}

/**
 * @brief 将16字节的UUID数组转换为标准字符串格式
 * 
 * 该函数将一个包含16个字节的UUID数组转换为标准的UUID字符串表示形式，
 * 格式为8-4-4-4-12的十六进制字符串（例如：xxxxxxxx-xxxx-xxxx-xxxx-xxxxxxxxxxxx）。
 * 
 * @param uuid 包含16个字节的UUID数据的数组
 * @return std::string 返回格式化的UUID字符串
 */
std::string uuid_to_string(const std::array<uint8_t, 16>& uuid) {
    std::stringstream ss;
    ss << std::hex << std::setfill('0');
    for (size_t i = 0; i < uuid.size(); ++i) {
        // 在指定位置插入连字符以符合标准UUID格式
        if (i == 4 || i == 6 || i == 8 || i == 10) {
            ss << '-';
        }
        ss << std::setw(2) << static_cast<int>(uuid[i]);
    }
    return ss.str();
}

/**
 * @brief 创建Action客户端并连接到Action服务器
 * 
 * 此函数负责创建与任务执行Action服务器的连接。它会根据当前命名空间
 * 构造服务器名称，并等待服务器变为可用状态。
 * 
 * 该函数会设置client_created_标志为true，表示客户端已创建。
 */
void TaskClientNode::createActionClient()
{
    LOG_INFO(" Creating action client, current namespace: {}", namespace_name_);
    
    std::string action_server_name = "execute_task";
    if (!namespace_name_.empty()) {
        // 直接使用命名空间，确保它以/开头
        std::string clean_namespace = namespace_name_;
        
        // 确保命名空间以/开头
        if (!clean_namespace.empty() && clean_namespace[0] != '/') {
            clean_namespace = "/" + clean_namespace;
        }
        
        // 移除末尾的斜杠（如果存在）
        if (!clean_namespace.empty() && clean_namespace.back() == '/') {
            clean_namespace.pop_back();
        }
        
        action_server_name = clean_namespace + "/execute_task";
    }
    
    LOG_INFO(" Final action server name: {}", action_server_name);
    
    action_client_ = rclcpp_action::create_client<TaskExecution>(this, action_server_name);
    
    // 增加等待时间并添加更详细的日志
    if (!action_client_->wait_for_action_server(std::chrono::seconds(20))) {
        LOG_ERROR(" Action server {} not available after waiting", action_server_name);
    } else {
        LOG_INFO(" Connected to action server {}", action_server_name);
    }
    
    client_created_ = true;
}

/**
 * @brief 设置命名空间并重新创建Action客户端
 * 
 * 此函数用于更新客户端的命名空间，并基于新的命名空间重新创建Action客户端。
 * 
 * @param namespace_name 新的命名空间名称
 */
void TaskClientNode::setNamespace(const QString& namespace_name)
{
    LOG_INFO(" setNamespace called with: {}", namespace_name.toStdString());
    namespace_name_ = namespace_name.toStdString();
    createActionClient();
}

/**
 * @brief 发送任务到Action服务器
 * 
 * 此函数用于向任务执行服务器发送任务请求。如果客户端尚未创建，
 * 它会先创建客户端。然后检查服务器是否可用，如果不可用则发出错误信号。
 * 最后构造任务目标消息并异步发送。
 * 
 * @param task_id 任务唯一标识符
 * @param task_type 任务类型 (如: inspection, harvest, weeding等)
 * @param action 操作类型 (如: start, pause, resume, cancel等)
 * @param task_start_location 任务开始位置
 * @param task_end_location 任务结束位置
 * @param priority 任务优先级（0-9，默认为5）
 * @param json_data 额外的JSON格式元数据（可选）
 * @param cmd_id 命令ID（默认为0）
 */
void TaskClientNode::sendTask(
    const QString& task_id,
    const QString& task_type,
    const QString& action,
    const QString& task_start_location,
    const QString& task_end_location,
    const QString& lobe_data,
    int priority,
    const QString& json_data,
    int cmd_id)
{
    LOG_INFO(" sendTaskWithLobeData called");
    
    // 如果客户端尚未创建，则创建它
    if (!client_created_) {
        LOG_INFO(" Client not created yet, creating now");
        createActionClient();
    }
    
    // 检查Action服务器是否可用
    if (!action_client_ || !action_client_->action_server_is_ready()) {
        LOG_ERROR(" Action server is not ready");
        Q_EMIT resultReceived(task_id, "failed", "Action server is not ready");
        return;
    }
    
    auto goal_msg = TaskExecution::Goal();
    goal_msg.task_id = task_id.toStdString();
    goal_msg.task_type = task_type.toStdString();
    goal_msg.action = action.toStdString();
    goal_msg.task_start_location = task_start_location.toStdString();
    goal_msg.task_end_location = task_end_location.toStdString();
    goal_msg.priority = priority;
    goal_msg.metadata = json_data.toStdString();
    goal_msg.cmd_id = cmd_id;
    
    // 添加lobe_data到goal_msg中
    goal_msg.lobe_data = lobe_data.toStdString();

    LOG_INFO(" Sending task : task_id={} task_type={} action={} cmd_id={} lobe_data={} cmd_id:{}", 
                  task_id.toStdString(), task_type.toStdString(), action.toStdString(), cmd_id, lobe_data.toStdString(),cmd_id);

    auto goal_options = rclcpp_action::Client<TaskExecution>::SendGoalOptions();
    goal_options.goal_response_callback = std::bind(&TaskClientNode::goalResponseCallback, this, std::placeholders::_1);
    goal_options.feedback_callback = std::bind(&TaskClientNode::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2);
    goal_options.result_callback = std::bind(&TaskClientNode::resultCallback, this, std::placeholders::_1);

    action_client_->async_send_goal(goal_msg, goal_options);
}

/**
 * @brief 处理目标响应的回调函数
 * 
 * 当服务器对发送的目标做出响应时调用此函数。如果目标被服务器拒绝，
 * 则发出resultReceived信号，状态为"failed"；如果目标被接受，则记录
 * 目标的UUID信息。
 * 
 * @param goal_handle 指向目标句柄的共享指针，如果为nullptr表示目标被拒绝
 */
void TaskClientNode::goalResponseCallback(GoalHandle::SharedPtr goal_handle)
{
    if (!goal_handle) {
        LOG_ERROR(" Goal was rejected by server");
        Q_EMIT resultReceived("", "failed", "Goal rejected");
    } else {
        // 使用辅助函数将UUID转换为字符串
        std::string goal_id_str = uuid_to_string(goal_handle->get_goal_id());
        LOG_INFO(" Goal accepted by server, goal ID: {}", goal_id_str);
    }
}

/**
 * @brief 处理反馈信息的回调函数
 * 
 * 当服务器发送任务执行过程中的反馈信息时调用此函数。将反馈信息中的
 * 数据转换为QString格式并记录日志，然后通过feedbackReceived信号发送出去。
 * 
 * @param goal_handle 指向目标句柄的共享指针
 * @param feedback 指向反馈信息的共享指针，包含任务执行过程中的实时状态
 */
void TaskClientNode::feedbackCallback(
    GoalHandle::SharedPtr,
    const std::shared_ptr<const TaskExecution::Feedback> feedback)
{
    auto fb = *feedback;
    QString q_task_id = QString::fromStdString(fb.task_id);
    QString q_status = QString::fromStdString(fb.current_status);
    QString q_subtask = QString::fromStdString(fb.active_subtask);
    LOG_INFO(" Feedback received: task_id={} status={} subtask={}", 
                  q_task_id.toStdString(), q_status.toStdString(), q_subtask.toStdString());
    Q_EMIT feedbackReceived(q_task_id, fb.progress_percentage, q_status, q_subtask);
}

/**
 * @brief 处理任务执行结果的回调函数
 * 
 * 当任务执行完成时调用此函数。根据执行结果代码判断任务是成功、中止
 * 还是被取消，并记录相应的日志信息。最后通过resultReceived信号发送结果。
 * 
 * @param result 包含任务执行结果的WrappedResult对象
 */
void TaskClientNode::resultCallback(const GoalHandle::WrappedResult& result)
{
    // 根据结果代码判断任务执行状态
    switch (result.code) {
        case rclcpp_action::ResultCode::SUCCEEDED:
            LOG_INFO(" Task succeeded");
            break;
        case rclcpp_action::ResultCode::ABORTED:
            LOG_ERROR(" Task aborted");
            break;
        case rclcpp_action::ResultCode::CANCELED:
            LOG_INFO(" Task was canceled");
            break;
        default:
            LOG_ERROR(" Unknown result code: {}", static_cast<int>(result.code));
            break;
    }

    // 提取结果数据并发送信号
    auto res = result.result;
    QString status = QString::fromStdString(res->status);
    QString message = QString::fromStdString(res->message);
    QString task_id = QString::fromStdString(res->task_id);
    LOG_INFO(" Result: task_id={} status={} message={}",
                  task_id.toStdString(), status.toStdString(), message.toStdString());
    Q_EMIT resultReceived(task_id, status, message);
}